/**
* @file        StaticAccChecker
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/7/31 15:20
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#include "StaticStateDetection.hpp"
#include "TransformCoordinate.hpp"
#include <iostream>

BEGIN_SPACE_LANGE_VDR
StaticStateDetection::StaticStateDetection(int bufflen, double init_acc_std_thres) {
    mBuffLength = bufflen;
    mAccStdThreshold = init_acc_std_thres;
    mVecAcc.resize(bufflen);
}

void StaticStateDetection::Reset() {

}

void StaticStateDetection::Insert(std::shared_ptr<NaviData::ImuData> imuData) {
    auto vec = imuData->getImuDataDetail().acce_;
    mVecAcc[mFixedImuSize % mBuffLength] = vec;
    mFixedImuSize++;
    mIsStatic =  CheckStatic();
}


bool StaticStateDetection::CheckStatic() {
    if(mFixedImuSize >= mBuffLength){
        double acc_var = 0.0;
        Eigen::Vector3d acc_mean = Eigen::Vector3d::Zero();
        CalVarianceAndMean(mVecAcc,  acc_mean, acc_var);
        float gravity_norm = acc_mean.norm();
        if(gravity_norm > 0.2f * G
           && gravity_norm < 1.8f * G
           && acc_var < mAccStdThreshold ){
            mInitRollPitch[0] = asin(acc_mean[1] /G) * (acc_mean[2] > 0? 1: -1);
            mInitRollPitch[1] = asin(acc_mean[0] /G) *  (acc_mean[2] > 0? -1:1);
            return true;
        }else{
            return false;
        }
    }
    return false;
}

void StaticStateDetection::CalVarianceAndMean(std::vector<Eigen::Vector3d> &vec_vec, Eigen::Vector3d &mean,
                                              double &variance) {

    mean.setZero();
    int size = vec_vec.size();
    for(auto i = 0; i < size; ++i){
        mean += vec_vec[i];
    }
    for(auto i = 0; i < 3; ++i){
        mean[i] /= size;
    }
    variance = 0.0;
    for(int i = 0; i < size; ++i){
        variance += ( vec_vec[i] - mean).transpose() * (vec_vec[i] - mean);
    }
    if(size > 1) variance /= (size - 1);
}

bool StaticStateDetection::IsStatic() {
    return mIsStatic;
}

Eigen::Vector2d StaticStateDetection::GetRollPitch() {
    return mInitRollPitch;
}
END_SPACE_LANGE_VDR